%% solvePQP.m
% Solve the problem using the PQP algorithm... in serial. 

loadProblem()

% options = optimset('Algorithm','interior-point-convex','TolFun',1e-6);

rand = 0;
[A,B,d,Hp,M] = makeMatrices(n,N,dt,rand);
tol = 1e-6;

% Put into single precision (default is double precision)
singleprec = 0;
if singleprec
    A{:} = single(A{:});
    B{:} = single(B{:});
    d = single(d);
    Hp = single(Hp);
    M = single(M);
end

% turn plots on
plotOn = 1;

%% Dual Problem Set up
AQP = M;
HQP = Hp;
QDP = AQP*HQP^-1*AQP';
VQinv = AQP*HQP^-1;
Qinv = HQP^-1;

%% Dual Problem solve

[numduals,numduals] = size(QDP);
lambda0 = ones(numduals,1);

if plotOn
    % Initialize plots
    figure(1)
    plot(1:tendtemp,Tlo*ones(tendtemp),'r--')
    hold on
    plot(1:tendtemp,Thi*ones(tendtemp),'r--')
    title('Temperature')
    figure(2)
    plot(1:tendtemp,ulo*ones(tendtemp),'r--')
    hold on 
    plot(1:tendtemp,uhi*ones(tendtemp),'r--')
    title('Control')
end

% Initialize the simulation
x0 = T0; 
U = []; T = T0;
for t = 1:tendtemp

    % Make whi and wlo
    whi = zeros(4*n*(N-1),1);
    wlo = zeros(4*n*(N-1),1);
    % for zone = 1:n
       for timestep = 1:N-1
          t1 = (timestep-1)*3+1;
          t3 = ((timestep-1)*3+1+2);
          assert(t3-t1 == 2);
          whi(t1:t3) = Thi*ones(3,1) - A{1}^timestep*x0;
          wlo(t1:t3) = Tlo*ones(3,1) - A{1}^timestep*x0;
          for k = 1:timestep
              whi(t1:t3) = whi(t1:t3)- A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
              wlo(t1:t3) = wlo(t1:t3) - A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
          end
       end
    % end
    whi(3*n*(N-1)+1:end) = uhi*ones(n*(N-1),1);
    wlo(3*n*(N-1)+1:end) = ulo*ones(n*(N-1),1);
    assert(length(whi)==4*n*(N-1),'whi wrong length');
    assert(length(wlo)==4*n*(N-1),'wlo wrong length');
    if singleprec
       whi = single(whi);
       wlo = single(wlo);
    end
    
    bQP = [whi;-wlo];
    
    if singleprec
        bQP = single(bQP);
    end
%     if t==1 
%         disp('bQP is...')
%         bQP
%     end
    
    HDP = bQP;% + VQinv*fQP;

    % Solve the quad prog!!
    tic
    [lambda,fval] = PQP(QDP,HDP,lambda0,t);
    u = -Qinv*(AQP'*lambda);
%     lambda0 = lambda;
    solvertime = toc

    x = zeros(3,N);
    x(:,1) = x0;
    for k = 1:N-1
        x(:,k+1) = A{1}*x(:,k)+B{1}*u(k)+[d(mod(t+k-1,tend));0;0];
    end

    if plotOn
        % Update plots with MPC open loop
        figure(1)
        plot(t+(1:N),x(1,:),'k--');
        figure(2)
        plot(t+(1:N-1),u,'k--');
    end
    
    
    % Update x0
%     x0 = A*x0+B*u(1)+d(mod(t,tend));
    x0 = x(:,2);
    u0 = u(1);
    
    % Print x
    disp(['iteration ' num2str(t) ', x = ' num2str(x0(1))]);
    disp(['iteration ' num2str(t) ', u0 = ' num2str(u0)]);
    disp(['iteration ' num2str(t) ', cost = ' num2str(fval)]);
%     if t >=99
%         u
%     end
    
    % Store used variables
    T = [T,x0];
    U = [U;u0];
end

